/*
    Copyright (c) 2011 Marcus Schwartz.  All rights reserved.
 
    This file is part of NetObjects-Arduino.

    NetObjects-Arduino is free software: you can redistribute it and/or 
    modify it under the terms of the GNU Lesser General Public License as 
    published by the Free Software Foundation, either version 3 of the 
    License, or (at your option) any later version.

    NetObjects-Arduino is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public 
    License along with NetObjects-Arduino.  If not, see 
    <http://www.gnu.org/licenses/>.
 */

#include <AFMotor.h>
#include <math.h>
#include <NetObjects.h>
#include <XBee.h>

#define VELOCITY 254 // max speed

AF_DCMotor m_driver(1, MOTOR12_64KHZ); // driver-side motor
AF_DCMotor m_passenger(2, MOTOR12_64KHZ); // passenger-side motor

NetObjectsControl net;
TwoButtonJoystick joystick(&net);
SkidSteerVehicle vehicle(&net);

void setup() {
  net.begin(19200);
  delay(200);
  net.debug("VehSt");
}

void loop() {
  NetObjectsUpdate update = net.getUpdate();
  if (update.type() == MOTION_TWO_BUTTON_JOYSTICK)
    joystick = update.twoButtonJoystick();
    
  vehicle.updateFromTwoButtonJoystick(joystick);
  
  set_motors(vehicle.speed_l(), vehicle.speed_r());
  
  vehicle.heading(1234);
  vehicle.rssi(net.getRssi());
  
  vehicle.broadcast();
}

void set_motors(int d_spd, int p_spd) {
  m_driver.setSpeed(abs(d_spd));
  m_passenger.setSpeed(abs(p_spd));
  
  if (d_spd > 20) 
    m_driver.run(FORWARD);
  else if (d_spd < -20)
    m_driver.run(BACKWARD);
  else
    m_driver.run(RELEASE);

  if (p_spd > 20)
    m_passenger.run(FORWARD);
  else if (p_spd < -20)
    m_passenger.run(BACKWARD);
  else
    m_passenger.run(RELEASE);
}
